#include <algorithm/math.h>
#include <units.h>

#define ATAN2_NB_COEFS_F32 10
#define PIHALFF32 1.5707963267948966192313f
#define FAST_MATH_TABLE_SIZE 512

static float32_t const atan2_coefs_f32[ATAN2_NB_COEFS_F32] = {0.0F,
                                                              1.0000001638308195518F,
                                                              -0.0000228941363602264F,
                                                              -0.3328086544578890873F,
                                                              -0.004404814619311061F,
                                                              0.2162217461808173258F,
                                                              -0.0207504842057097504F,
                                                              -0.1745263362250363339F,
                                                              0.1340557235283553386F,
                                                              -0.0323664125927477625f};

constexpr float32_t sinTable[FAST_MATH_TABLE_SIZE + 1] = {
    0.00000000F, 0.01227154F, 0.02454123F, 0.03680722F, 0.04906767F, 0.06132074F, 0.07356456F, 0.08579731F,
    0.09801714F, 0.11022221F, 0.12241068F, 0.13458071F, 0.14673047F, 0.15885814F, 0.17096189F, 0.18303989F,
    0.19509032F, 0.20711138F, 0.21910124F, 0.23105811F, 0.24298018F, 0.25486566F, 0.26671276F, 0.27851969F,
    0.29028468F, 0.30200595F, 0.31368174F, 0.32531029F, 0.33688985F, 0.34841868F, 0.35989504F, 0.37131719F,
    0.38268343F, 0.39399204F, 0.40524131F, 0.41642956F, 0.42755509F, 0.43861624F, 0.44961133F, 0.46053871F,
    0.47139674F, 0.48218377F, 0.49289819F, 0.50353838F, 0.51410274F, 0.52458968F, 0.53499762F, 0.54532499F,
    0.55557023F, 0.56573181F, 0.57580819F, 0.58579786F, 0.59569930F, 0.60551104F, 0.61523159F, 0.62485949F,
    0.63439328F, 0.64383154F, 0.65317284F, 0.66241578F, 0.67155895F, 0.68060100F, 0.68954054F, 0.69837625F,
    0.70710678F, 0.71573083F, 0.72424708F, 0.73265427F, 0.74095113F, 0.74913639F, 0.75720885F, 0.76516727F,
    0.77301045F, 0.78073723F, 0.78834643F, 0.79583690F, 0.80320753F, 0.81045720F, 0.81758481F, 0.82458930F,
    0.83146961F, 0.83822471F, 0.84485357F, 0.85135519F, 0.85772861F, 0.86397286F, 0.87008699F, 0.87607009F,
    0.88192126F, 0.88763962F, 0.89322430F, 0.89867447F, 0.90398929F, 0.90916798F, 0.91420976F, 0.91911385F,
    0.92387953F, 0.92850608F, 0.93299280F, 0.93733901F, 0.94154407F, 0.94560733F, 0.94952818F, 0.95330604F,
    0.95694034F, 0.96043052F, 0.96377607F, 0.96697647F, 0.97003125F, 0.97293995F, 0.97570213F, 0.97831737F,
    0.98078528F, 0.98310549F, 0.98527764F, 0.98730142F, 0.98917651F, 0.99090264F, 0.99247953F, 0.99390697F,
    0.99518473F, 0.99631261F, 0.99729046F, 0.99811811F, 0.99879546F, 0.99932238F, 0.99969882F, 0.99992470F,
    1.00000000F, 0.99992470F, 0.99969882F, 0.99932238F, 0.99879546F, 0.99811811F, 0.99729046F, 0.99631261F,
    0.99518473F, 0.99390697F, 0.99247953F, 0.99090264F, 0.98917651F, 0.98730142F, 0.98527764F, 0.98310549F,
    0.98078528F, 0.97831737F, 0.97570213F, 0.97293995F, 0.97003125F, 0.96697647F, 0.96377607F, 0.96043052F,
    0.95694034F, 0.95330604F, 0.94952818F, 0.94560733F, 0.94154407F, 0.93733901F, 0.93299280F, 0.92850608F,
    0.92387953F, 0.91911385F, 0.91420976F, 0.90916798F, 0.90398929F, 0.89867447F, 0.89322430F, 0.88763962F,
    0.88192126F, 0.87607009F, 0.87008699F, 0.86397286F, 0.85772861F, 0.85135519F, 0.84485357F, 0.83822471F,
    0.83146961F, 0.82458930F, 0.81758481F, 0.81045720F, 0.80320753F, 0.79583690F, 0.78834643F, 0.78073723F,
    0.77301045F, 0.76516727F, 0.75720885F, 0.74913639F, 0.74095113F, 0.73265427F, 0.72424708F, 0.71573083F,
    0.70710678F, 0.69837625F, 0.68954054F, 0.68060100F, 0.67155895F, 0.66241578F, 0.65317284F, 0.64383154F,
    0.63439328F, 0.62485949F, 0.61523159F, 0.60551104F, 0.59569930F, 0.58579786F, 0.57580819F, 0.56573181F,
    0.55557023F, 0.54532499F, 0.53499762F, 0.52458968F, 0.51410274F, 0.50353838F, 0.49289819F, 0.48218377F,
    0.47139674F, 0.46053871F, 0.44961133F, 0.43861624F, 0.42755509F, 0.41642956F, 0.40524131F, 0.39399204F,
    0.38268343F, 0.37131719F, 0.35989504F, 0.34841868F, 0.33688985F, 0.32531029F, 0.31368174F, 0.30200595F,
    0.29028468F, 0.27851969F, 0.26671276F, 0.25486566F, 0.24298018F, 0.23105811F, 0.21910124F, 0.20711138F,
    0.19509032F, 0.18303989F, 0.17096189F, 0.15885814F, 0.14673047F, 0.13458071F, 0.12241068F, 0.11022221F,
    0.09801714F, 0.08579731F, 0.07356456F, 0.06132074F, 0.04906767F, 0.03680722F, 0.02454123F, 0.01227154F,
    0.00000000F, -0.01227154F, -0.02454123F, -0.03680722F, -0.04906767F, -0.06132074F, -0.07356456F, -0.08579731F,
    -0.09801714F, -0.11022221F, -0.12241068F, -0.13458071F, -0.14673047F, -0.15885814F, -0.17096189F, -0.18303989F,
    -0.19509032F, -0.20711138F, -0.21910124F, -0.23105811F, -0.24298018F, -0.25486566F, -0.26671276F, -0.27851969F,
    -0.29028468F, -0.30200595F, -0.31368174F, -0.32531029F, -0.33688985F, -0.34841868F, -0.35989504F, -0.37131719F,
    -0.38268343F, -0.39399204F, -0.40524131F, -0.41642956F, -0.42755509F, -0.43861624F, -0.44961133F, -0.46053871F,
    -0.47139674F, -0.48218377F, -0.49289819F, -0.50353838F, -0.51410274F, -0.52458968F, -0.53499762F, -0.54532499F,
    -0.55557023F, -0.56573181F, -0.57580819F, -0.58579786F, -0.59569930F, -0.60551104F, -0.61523159F, -0.62485949F,
    -0.63439328F, -0.64383154F, -0.65317284F, -0.66241578F, -0.67155895F, -0.68060100F, -0.68954054F, -0.69837625F,
    -0.70710678F, -0.71573083F, -0.72424708F, -0.73265427F, -0.74095113F, -0.74913639F, -0.75720885F, -0.76516727F,
    -0.77301045F, -0.78073723F, -0.78834643F, -0.79583690F, -0.80320753F, -0.81045720F, -0.81758481F, -0.82458930F,
    -0.83146961F, -0.83822471F, -0.84485357F, -0.85135519F, -0.85772861F, -0.86397286F, -0.87008699F, -0.87607009F,
    -0.88192126F, -0.88763962F, -0.89322430F, -0.89867447F, -0.90398929F, -0.90916798F, -0.91420976F, -0.91911385F,
    -0.92387953F, -0.92850608F, -0.93299280F, -0.93733901F, -0.94154407F, -0.94560733F, -0.94952818F, -0.95330604F,
    -0.95694034F, -0.96043052F, -0.96377607F, -0.96697647F, -0.97003125F, -0.97293995F, -0.97570213F, -0.97831737F,
    -0.98078528F, -0.98310549F, -0.98527764F, -0.98730142F, -0.98917651F, -0.99090264F, -0.99247953F, -0.99390697F,
    -0.99518473F, -0.99631261F, -0.99729046F, -0.99811811F, -0.99879546F, -0.99932238F, -0.99969882F, -0.99992470F,
    -1.00000000F, -0.99992470F, -0.99969882F, -0.99932238F, -0.99879546F, -0.99811811F, -0.99729046F, -0.99631261F,
    -0.99518473F, -0.99390697F, -0.99247953F, -0.99090264F, -0.98917651F, -0.98730142F, -0.98527764F, -0.98310549F,
    -0.98078528F, -0.97831737F, -0.97570213F, -0.97293995F, -0.97003125F, -0.96697647F, -0.96377607F, -0.96043052F,
    -0.95694034F, -0.95330604F, -0.94952818F, -0.94560733F, -0.94154407F, -0.93733901F, -0.93299280F, -0.92850608F,
    -0.92387953F, -0.91911385F, -0.91420976F, -0.90916798F, -0.90398929F, -0.89867447F, -0.89322430F, -0.88763962F,
    -0.88192126F, -0.87607009F, -0.87008699F, -0.86397286F, -0.85772861F, -0.85135519F, -0.84485357F, -0.83822471F,
    -0.83146961F, -0.82458930F, -0.81758481F, -0.81045720F, -0.80320753F, -0.79583690F, -0.78834643F, -0.78073723F,
    -0.77301045F, -0.76516727F, -0.75720885F, -0.74913639F, -0.74095113F, -0.73265427F, -0.72424708F, -0.71573083F,
    -0.70710678F, -0.69837625F, -0.68954054F, -0.68060100F, -0.67155895F, -0.66241578F, -0.65317284F, -0.64383154F,
    -0.63439328F, -0.62485949F, -0.61523159F, -0.60551104F, -0.59569930F, -0.58579786F, -0.57580819F, -0.56573181F,
    -0.55557023F, -0.54532499F, -0.53499762F, -0.52458968F, -0.51410274F, -0.50353838F, -0.49289819F, -0.48218377F,
    -0.47139674F, -0.46053871F, -0.44961133F, -0.43861624F, -0.42755509F, -0.41642956F, -0.40524131F, -0.39399204F,
    -0.38268343F, -0.37131719F, -0.35989504F, -0.34841868F, -0.33688985F, -0.32531029F, -0.31368174F, -0.30200595F,
    -0.29028468F, -0.27851969F, -0.26671276F, -0.25486566F, -0.24298018F, -0.23105811F, -0.21910124F, -0.20711138F,
    -0.19509032F, -0.18303989F, -0.17096189F, -0.15885814F, -0.14673047F, -0.13458071F, -0.12241068F, -0.11022221F,
    -0.09801714F, -0.08579731F, -0.07356456F, -0.06132074F, -0.04906767F, -0.03680722F, -0.02454123F, -0.01227154F,
    -0.00000000f};

__attribute__((always_inline)) static __inline float32_t arm_atan_limited_f32(float32_t x) {
    float32_t res = atan2_coefs_f32[ATAN2_NB_COEFS_F32 - 1];
    int i = 1;
    for (i = 1; i < ATAN2_NB_COEFS_F32; i++) {
        res = x * res + atan2_coefs_f32[ATAN2_NB_COEFS_F32 - 1 - i];
    }

    return (res);
}

namespace os {
namespace math {
using namespace units::angle;

void sinCos(radian_t angle, float32_t* pSin, float32_t* pCos) {
    float32_t fract, in; /* Temporary input, output variables */
    uint16_t indexS, indexC; /* Index variable */
    float32_t f1, f2, d1, d2; /* Two nearest output values */
    float32_t Dn, Df;
    float32_t temp, findex;

    /* input x is in degrees */
    /* Scale input, divide input by 360, for cosine add 0.25 (pi/2) to read sine
     * table */
    in = angle() * 0.00277777777778f;

    if (in < 0.0f) {
        in = -in;
    }

    in = in - (int32_t)in;

    /* Calculate the nearest index */
    findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
    indexS = ((uint16_t)findex) & 0x1ff;
    indexC = (indexS + (FAST_MATH_TABLE_SIZE / 4)) & 0x1ff;

    /* Calculation of fractional value */
    fract = findex - (float32_t)indexS;

    /* Read two nearest values of input value from the cos & sin tables */
    f1 = sinTable[indexC];
    f2 = sinTable[indexC + 1];
    d1 = -sinTable[indexS];
    d2 = -sinTable[indexS + 1];

    Dn = 0.0122718463030f; /* delta between the two points (fixed), in this case
                            2*pi/FAST_MATH_TABLE_SIZE */
    Df = f2 - f1; /* delta between the values of the functions */

    temp = Dn * (d1 + d2) - 2 * Df;
    temp = fract * temp + (3 * Df - (d2 + 2 * d1) * Dn);
    temp = fract * temp + d1 * Dn;

    /* Calculation of cosine value */
    *pCos = fract * temp + f1;

    /* Read two nearest values of input value from the cos & sin tables */
    f1 = sinTable[indexS];
    f2 = sinTable[indexS + 1];
    d1 = sinTable[indexC];
    d2 = sinTable[indexC + 1];

    Df = f2 - f1; // delta between the values of the functions
    temp = Dn * (d1 + d2) - 2 * Df;
    temp = fract * temp + (3 * Df - (d2 + 2 * d1) * Dn);
    temp = fract * temp + d1 * Dn;

    /* Calculation of sine value */
    *pSin = fract * temp + f1;

    if (angle() < 0.0f) {
        *pSin = -*pSin;
    }
}

radian_t atan2(float32_t y, float32_t x) {
    using namespace units::literals;
    if (x > 0.0f) {
        return atan(y / x);
    }
    if (x < 0.0f) {
        if (y > 0.0f) {
            return atan(y / x) + radian_t(180_deg);
        }
        if (y < 0.0f) {
            return atan(y / x) - radian_t(180_deg);
        }
        if (std::signbit(y)) {
            return -180_deg;
        }
        return 180_deg;
    }
    if (x == 0.0f) {
        if (y > 0.0f) {
            return radian_t(PIHALFF32);
        }
        if (y < 0.0f) {
            return -radian_t(PIHALFF32);
        }
    }
    return radian_t(0);
}

void abs(float32_t* src, float32_t* dst, uint32_t size) {
    uint32_t blkCnt; /* Loop counter */

    /* Loop unrolling: Compute 4 outputs at a time */
    blkCnt = size >> 2U;

    while (blkCnt > 0U) {
        /* C = |A| */

        /* Calculate absolute and store result in destination buffer. */
        *dst++ = fabsf(*src++);

        *dst++ = fabsf(*src++);

        *dst++ = fabsf(*src++);

        *dst++ = fabsf(*src++);

        /* Decrement loop counter */
        blkCnt--;
    }

    /* Loop unrolling: Compute remaining outputs */
    blkCnt = size % 0x4U;

    while (blkCnt > 0U) {
        /* C = |A| */

        /* Calculate absolute and store result in destination buffer. */
        *dst++ = fabsf(*src++);

        /* Decrement loop counter */
        blkCnt--;
    }
}

void add(float32_t* src_a, float32_t* src_b, float32_t* dst, uint32_t size) {
    uint32_t blkCnt; /* Loop counter */

    blkCnt = size >> 2U;

    while (blkCnt > 0U) {
        /* C = A + B */

        /* Add and store result in destination buffer. */
        *dst++ = (*src_a++) + (*src_b++);
        *dst++ = (*src_a++) + (*src_b++);
        *dst++ = (*src_a++) + (*src_b++);
        *dst++ = (*src_a++) + (*src_b++);

        /* Decrement loop counter */
        blkCnt--;
    }

    /* Loop unrolling: Compute remaining outputs */
    blkCnt = size % 0x4U;

    while (blkCnt > 0U) {
        /* C = A + B */

        /* Add and store result in destination buffer. */
        *dst++ = (*src_a++) + (*src_b++);

        /* Decrement loop counter */
        blkCnt--;
    }
}

void sub(float32_t* src_a, float32_t* src_b, float32_t* dst, uint32_t size) {
    uint32_t blkCnt; /* Loop counter */

    blkCnt = size >> 2U;

    while (blkCnt > 0U) {
        /* C = A - B */

        /* Add and store result in destination buffer. */
        *dst++ = (*src_a++) - (*src_b++);
        *dst++ = (*src_a++) - (*src_b++);
        *dst++ = (*src_a++) - (*src_b++);
        *dst++ = (*src_a++) - (*src_b++);

        /* Decrement loop counter */
        blkCnt--;
    }

    /* Loop unrolling: Compute remaining outputs */
    blkCnt = size % 0x4U;

    while (blkCnt > 0U) {
        /* C = A + B */

        /* Add and store result in destination buffer. */
        *dst++ = (*src_a++) - (*src_b++);

        /* Decrement loop counter */
        blkCnt--;
    }
}

float32_t cos(radian_t angle) {
    float32_t cosVal, fract, in; /* Temporary input, output variables */
    uint16_t index; /* Index variable */
    float32_t a, b; /* Two nearest output values */
    int32_t n;
    float32_t findex;

    /* input x is in radians */
    /* Scale input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25
     * (pi/2) to read sine table */
    in = angle() * 0.159154943092f + 0.25f;

    /* Calculation of floor value of input */
    n = (int32_t)in;

    /* Make negative values towards -infinity */
    if (in < 0.0f) {
        n--;
    }

    /* Map input value to [0 1] */
    in = in - (float32_t)n;

    /* Calculation of index of the table */
    findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
    index = (uint16_t)findex;

    /* when "in" is exactly 1, we need to rotate the index down to 0 */
    if (index >= FAST_MATH_TABLE_SIZE) {
        index = 0;
        findex -= (float32_t)FAST_MATH_TABLE_SIZE;
    }

    /* fractional value calculation */
    fract = findex - (float32_t)index;

    /* Read two nearest values of input value from the cos table */
    a = sinTable[index];
    b = sinTable[index + 1];

    /* Linear interpolation process */
    cosVal = (1.0f - fract) * a + fract * b;

    /* Return output value */
    return (cosVal);
}

float32_t sin(radian_t angle) {
    float32_t sinVal, fract, in; /* Temporary input, output variables */
    uint16_t index; /* Index variable */
    float32_t a, b; /* Two nearest output values */
    int32_t n;
    float32_t findex;

    /* input x is in radians */
    /* Scale input to [0 1] range from [0 2*PI] , divide input by 2*pi */
    in = angle() * 0.159154943092f;

    /* Calculation of floor value of input */
    n = (int32_t)in;

    /* Make negative values towards -infinity */
    if (in < 0.0f) {
        n--;
    }

    /* Map input value to [0 1] */
    in = in - (float32_t)n;

    /* Calculation of index of the table */
    findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
    index = (uint16_t)findex;

    /* when "in" is exactly 1, we need to rotate the index down to 0 */
    if (index >= FAST_MATH_TABLE_SIZE) {
        index = 0;
        findex -= (float32_t)FAST_MATH_TABLE_SIZE;
    }

    /* fractional value calculation */
    fract = findex - (float32_t)index;

    /* Read two nearest values of input value from the sin table */
    a = sinTable[index];
    b = sinTable[index + 1];

    /* Linear interpolation process */
    sinVal = (1.0f - fract) * a + fract * b;

    /* Return output value */
    return (sinVal);
}

float32_t tan(radian_t angle) { return sin(angle) / cos(angle); }

radian_t atan(float32_t x) {
    int sign = 0;
    float32_t res = 0.0f;
    if (x < 0.0f) {
        sign = 1;
        x = -x;
    }
    if (x > 1.0F) {
        x = 1.0F / x;
        res = PIHALFF32 - arm_atan_limited_f32(x);
    } else {
        res += arm_atan_limited_f32(x);
    }
    if (sign) {
        res = -res;
    }
    return radian_t(res);
}

radian_t asin(float32_t x) {
    float rd = asinf(x);
    return radian_t(rd);
}

float32_t invSqrt(float32_t x) {
    float32_t halfx = 0.5F * x;
    float32_t y = x;
    int32_t i = *reinterpret_cast<int32_t*>(&y);
    i = 0x5f3759df - (i >> 1);
    y = *reinterpret_cast<float32_t*>(&i);
    y = y * (1.5F - (halfx * y * y));
    return y;
}

int8_t sign(float32_t x) {
    if (x > 0) {
        return 1;
    }
    if (x < 0) {
        return -1;
    }
    return 0;
}

float32_t sqrt(float32_t x) {
    if (x >= 0.0F) {
        return sqrtf(x);
    }
    return NAN;
}

int16_t buff2i16(uint8_t const* buff) { return *(reinterpret_cast<int16_t const*>(buff)); }
float buff2float(uint8_t const* buff) { return *(reinterpret_cast<float const*>(buff)); }
void ui322buff(uint32_t u, uint8_t* buff) { *(reinterpret_cast<uint32_t*>(buff)) = u; }

/**
 * @brief      非线性函数
 * @param e
 * @param alpha
 * @param zeta
 * @param      NULL
 * @retval     result
 */
float fal(float const e, float const alpha, float const zeta) {
    int16_t s = 0;
    float fal_output = 0;
    s = (sign(e + zeta) - sign(e - zeta)) / 2;
    fal_output = e * s / (powf(zeta, 1 - alpha)) + powf(fabs(e), alpha) * sign(e) * (1 - s);
    return fal_output;
}

/**
 * @brief      计算非线性函数
 * @param      x :Number to be calc
 * @retval     result
 */
int16_t fsg(float const x, float const d) {
    int16_t output = 0;
    output = (sign(x + d) - sign(x - d)) / 2;
    return output;
}

/**
 * @brief      Calculation differential (only two order)(To be improved)
 * @param      arr :point to be differential value
 * @param      order :The differential order
 * @retval     NULL
 */
float differential(float const arr[], uint8_t const order) {
    float value = 0.0f;
    switch (order) {
        case 1:
            value = arr[0] - arr[1];
            break;
        case 2:
            value = arr[2] - 2 * arr[1] + arr[0];
            break;
        default:
            value = arr[0];
            break;
    }
    return value;
}

/**
 * @brief 将角度归一化到指定范围
 * @param angle 输入角度
 * @param min_angle 范围最小值
 * @param max_angle 范围最大值
 * @return 归一化后的角度
 */
} // namespace math
} // namespace os

namespace os {
namespace math {
void MathErrorHandler(char const* msg) {
    (void)msg;
#ifdef MathErrorLoop
  while (true) {
    // TODO: ...
  }
#endif
}
} // namespace math
} // namespace os